#该代码仅适用于2022ti十月电赛，
#部分注释为2车代码
#通过设置roi区域检测十字中的直线并计数，判断车辆位移的位置，是否要执行倒车，侧翻等动作
#通过串口于STM32通信，通信协议为借鉴版
import sensor, image, time, math,pyb, struct
from pyb import Pin, Timer,UART,LED
from image import SEARCH_EX, SEARCH_DS
import math
uart = pyb.UART(3,9600,timeout_char = 1000)#串口初始化

# Tracks a black line. Use [(128, 255)] for a tracking a white line.
GRAYSCALE_THRESHOLD = [(15, 100, -60, 7, -47, 30) ]

#ROIS = [                      #[ROI, weight]越近，权重越大，在这里权值暂时不考虑
#        (7, 22,   50, 8, 0),#下面1
#        (7, 12,   50, 8, 0),#中间2
#        (7, 1,    50, 8, 0),#上面3
#        (0, 0,    8, 31, 0),#左边4
#        (56,0 ,   8, 31, 0) #右边5
#       ]

ROIS = [                            #[ROI, weight]越近，权重越大，在这里权值暂时不考虑
               (0, 34,   128, 8, 0), #下面1
               (0, 28,   128, 8, 0), #中间2
               (0, 0,    100, 8, 0), #上面3
               (115, 0,  13, 8, 0), #十字1
               (108, 8,  10, 8, 0),
               (72,37,51,23,0)

       ]

weight_sum = 0
for r in ROIS: weight_sum += r[4] # r[4] is the roi weight.


is_debug = True
#----------------------------------------寻找直线变量----------------------------------------#
enable_lens_corr = False # turn on for straighter lines...
min_degree = 0
max_degree = 179
a1=0;
b1=0;
c1=0;
a2=0;
b2=0;
c2=0;
last_x=0;
last_y=0;
#---------------------------------------摄像头初始化-----------------------------------------#

sensor.reset()
sensor.set_contrast(1)
sensor.set_gainceiling(16)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.B128X64)                  # 颜色追踪:160*120  光流检测:40*30
sensor.skip_frames(time=2000)
sensor.set_auto_gain(False)                         # 颜色追踪关闭自动增益
sensor.set_auto_whitebal(False)                     # 颜色追踪关闭白平衡
Sum_x=0;
Sum_y=0;
flag=0;
i=0;  #记录第几行数据
j=0;  #记录直线数量
led=pyb.LED(3)#必要的时候进行红外补光
#检测圆形中心点的坐标
#十字计数
last_flag=0;
last_flag1=0;
num=0;
start=0;
#偏移
pos=0;
#
angle=0;
center_pos=0;
center_x=0;
center_y=0;
center_update=1;#中心圆位置更新的标志
center_x_old=0;
center_y_old=0;
center_pos_old=64;

center_x_down=0;
center_y_down=0;

center_x_up=0;
center_y_up=0;

center_x_mid=0;
center_y_mid=0;

center_y_left=0;
center_x_left=0;

center_y_right=0;
center_x_right=0;

center_flag1=0;#上下
center_flag2=0;#左右
center_flag3=0;#通过roll来调整黑线的位置  通过yaw来调整机头方向  矩形1和2=0;
center_flag4=0;
center_flag5=0;

turn_flag=0;#转弯的标志
a=0
m=0
b=0
s=0
yaw_angle=0;
out_str1='';
clock = time.clock()
#帧率
#old_img= sensor.snapshot()        # 160x120 -> 40x30,前一张照片
#定义一个定时发送数据的函数
def tick(timer):#we will receive the timer object when being called
        global flag
        flag=1
tim = Timer(4,freq=20)            # create a timer object using timer 4 - trigger at 1Hz
tim.callback(tick)                # set the callback to our tick function
#--------------------------------------发送协议-----------------------------------------#
#定义发送协议
def pack_bin_data(start,pos,num):
    '''
    通信协议，打包二进制数
    < 代表的是字节顺序(倒序) 跟Arduino有关.
    '''
    bin_data = struct.pack("BBhhhB",
            0xAA,
            0xAE,
            int(start),
            int(pos),
            int(num),
            0xBA)
    return bin_data
#定义接受协议
rx_buff=[]
state = 0
tx_flag = 0
def Receive_Prepare(data):
    global state
    global tx_flag
    if state==0:
        if data == 0x0d:#帧头
            state = 1
        else:
            state = 0
            rx_buff.clear()
    elif state==1:
        rx_buff.append(data)
        state = 2
    elif state==2:
        rx_buff.append(data)
        state = 3
    elif state == 3:
        if data == 0x5b:
            tx_flag = int(rx_buff[0])
            state = 4
    else:
        state = 0
        rx_buff.clear()
#--------------------------------------while循环开始-----------------------------------------#
while(True):

    if(flag==1):
        img=sensor.snapshot()
        img_old=img.copy()
        #--------------------------------------光流定点-----------------------------------------#
        #img.lens_corr(1.5) # for 2.8mm lens...摄像头畸变纠正
        #img.binary(GRAYSCALE_THRESHOLD);

        #--------------------------------------寻找黑线和圆心的位置--------------------------------------#
        centroid_sum=0#暂时用不到这个变量

        #检测圆形位置
        for r in ROIS:
            i=i+1;
            blobs=img_old.find_blobs(GRAYSCALE_THRESHOLD, invert=True, roi=r[0:4], merge=True,pixels_area=40) # r[0:4] is roi tuple.
            if blobs:#如果找到了颜色块
                # Find the blob with the most pixels.
                largest_blob = max(blobs, key=lambda b: b.pixels())
                img.draw_rectangle(largest_blob.rect())
                if(i==1):#下面矩形
                            center_x_down=largest_blob.cx();
                            center_y_down=largest_blob.cy();
                            center_flag1=1;#下面的矩形找到的标志


                elif(i==2):#中间矩形
                            center_x_mid=largest_blob.cx();
                            center_y_mid=largest_blob.cy();
                            center_flag2=1;

                elif(i==3):#上面的矩形
                            center_x_up=largest_blob.cx();
                            center_y_up=largest_blob.cy();
                            center_flag3=1;

                elif(i==4):#左边的矩形找到了
                            center_y_left=largest_blob.cy();
                            center_x_left=largest_blob.cx();
                            center_flag4=1;
                            last_flag=1;
                elif(i==5):#左边的矩形找到了
                            center_y_left=largest_blob.cy();
                            center_x_left=largest_blob.cx();
                            center_flag5=1;
                            last_flag1=1;
                elif(i==6):
                            center_x_roi=largest_blob.cx();
        if(last_flag==1 and last_flag1==1):
            if(center_flag4==0 and center_flag5==0):
                                                    last_flag=0;
                                                    last_flag1=0;
                                                    num=num+1;
        '''
        if(last_flag==1):
            if(center_flag4==0):
                                last_flag=0;
                                num=num+1;
        '''
        #print(num)
       # print(center_x_up,center_x_mid,center_x_down)

        #pos=int((center_x_up+center_x_down)/2-90)
        pos=center_x_up-90
        if(center_flag3==0):
                            pos=10;



       # print(last_x,last_y)
        #3开始检测直角
        #if(center_flag3==0 and center_flag1==1 and center_flag4==1):

      #  elif(num==11):
       #               start=2;
        center_flag1=0;#标志清零
        center_flag2=0;#标志清零
        center_flag3=0;
        center_flag4=0;
        center_flag5=0;
        i=0;
        #50ms发送一次数据到飞控

        out_str1+='%d'%      int(center_pos);
        #uart.write('['+int(out_str1)+']')
        start=center_x_roi-96
        print(start,pos,num)
        uart.write(pack_bin_data(start,int(pos),num))
        #像素位移之和清零
        pos=0
        angle=0
        center_pos=0;
        turn_flag=0;
        Sum_x=0
        Sum_y=0
        k=0
        n=0
        m=0
        start=0
        #print("%0.1f Xcm  %0.1f Ycm  %0.2fQ\t" %(Sum_x,Sum_y,response))
        #数组清零
        out_str1=''#清除之前的数据
        flag=0;
        #-----------------------------------串口打印数据-----------------------------------------#
        #以下为暂待代码
        '''
        #--------------------------------------检测直线交点的位置--------------------------------------#
        yaw_angle=0;·
        lines = img_old.find_lines(threshold=1000, theta_margin = 50, rho_margin = 50)
        for i in range(0,len(lines)-1):
            for j in range(i+1,len(lines)):
                l0x1 = lines[i].x1()
                l0x2 = lines[i].x2()
                l0y2 = lines[i].y2()
                l0y1 = lines[i].y1()
                if(l0x1 == l0x2):
                    l0x1 = l0x1 + 0.1
                a0 = (l0y2 - l0y1)/(l0x2 - l0x1)
                b0 = l0y1 - a0*l0x1
                l1x1 = lines[j].x1()
                l1y1 = lines[j].y1()
                l1x2 = lines[j].x2()
                l1y2 = lines[j].y2()
                if(l1x1 == l1x2):
                    l1x1 = l1x1 + 0.1
                a1 = (l1y2 - l1y1)/(l1x2 - l1x1)
                b1 = l1y1 - a1*l1x1
                if(a0==a1):
                    a0 = a0+ 0.1
                intersectionx = (b1-b0)/(a0-a1)
                intersectiony = a0*intersectionx + b0
                if((intersectionx-last_x)>2 or (intersectionx-last_x)<-2):
                    last_x=intersectionx;
                if((intersectiony-last_y)>2 or (intersectiony-last_y)<-2):
                    last_y=intersectiony;
        for l in lines:#画出所有的直线
          #img.draw_line(l.line())
          angle=l.theta()
          if(angle>90):
              angle=180-angle;
        #img.draw_cross(int(last_x), int(last_y),3,color=0)
        #img.draw_circle(int(last_x), int(last_y),3,color=0)
        '''


